/*
 * matrix.c
 *
 *  Created on: Jul 27, 2013
 *      Author: derekmorris
 */

#include <engine.h>

void matrixFrustum(Matrix4x4* pMatrix, float left, float right, float bottom, float top, float nearZ, float farZ)
{
	float deltaX = right - left;
	float deltaY = top - bottom;
	float deltaZ = farZ - nearZ;

	if ((nearZ <= 0.0f) || (farZ <= 0.0f) || (deltaX <= 0.0f)
			|| (deltaY <= 0.0f) || (deltaZ <= 0.0f))
		return;

	pMatrix->elements[0 * 4 + 0] = 2.0f * nearZ / deltaX;
	pMatrix->elements[0 * 4 + 1] = pMatrix->elements[0 * 4 + 2] = pMatrix->elements[0 * 4 + 3] = 0.0f;

	pMatrix->elements[1 * 4 + 1] = 2.0f * nearZ / deltaY;
	pMatrix->elements[1 * 4 + 0] = pMatrix->elements[1 * 4 + 2] = pMatrix->elements[1 * 4 + 3] = 0.0f;

	pMatrix->elements[2 * 4 + 0] = (right + left) / deltaX;
	pMatrix->elements[2 * 4 + 1] = (top + bottom) / deltaY;
	pMatrix->elements[2 * 4 + 2] = -(nearZ + farZ) / deltaZ;
	pMatrix->elements[2 * 4 + 3] = -1.0f;

	pMatrix->elements[3 * 4 + 2] = -2.0f * nearZ * farZ / deltaZ;
	pMatrix->elements[3 * 4 + 0] = pMatrix->elements[3 * 4 + 1] = pMatrix->elements[3 * 4 + 3] = 0.0f;
}

void matrixPerspective(Matrix4x4* pMatrix, float fovy, float aspect, float nearZ, float farZ)
{
	float frustumW, frustumH;

	frustumH = (float) tanf(fovy / 360.0 * M_PI) * nearZ;
	frustumW = frustumH * aspect;

	matrixFrustum(pMatrix, -frustumW, frustumW, -frustumH, frustumH, nearZ, farZ);
}

void matrixRotate(Matrix4x4* pMatrix, float angle, float x, float y, float z)
{
	float sinAngle, cosAngle;
	float mag = (float) sqrt((double) (x * x + y * y + z * z));

	sinAngle = (float) sin((double) (angle * M_PI / 180.0));
	cosAngle = (float) cos((double) (angle * M_PI / 180.0));
	if (mag > 0.0f) {
		float xx, yy, zz, xy, yz, zx, xs, ys, zs;
		float oneMinusCos;

		x /= mag;
		y /= mag;
		z /= mag;

		xx = x * x;
		yy = y * y;
		zz = z * z;
		xy = x * y;
		yz = y * z;
		zx = z * x;
		xs = x * sinAngle;
		ys = y * sinAngle;
		zs = z * sinAngle;
		oneMinusCos = 1.0f - cosAngle;

		pMatrix->elements[0 * 4 + 0] = (oneMinusCos * xx) + cosAngle;
		pMatrix->elements[0 * 4 + 1] = (oneMinusCos * xy) - zs;
		pMatrix->elements[0 * 4 + 2] = (oneMinusCos * zx) + ys;
		pMatrix->elements[0 * 4 + 3] = 0.0F;

		pMatrix->elements[1 * 4 + 0] = (oneMinusCos * xy) + zs;
		pMatrix->elements[1 * 4 + 1] = (oneMinusCos * yy) + cosAngle;
		pMatrix->elements[1 * 4 + 2] = (oneMinusCos * yz) - xs;
		pMatrix->elements[1 * 4 + 3] = 0.0F;

		pMatrix->elements[2 * 4 + 0] = (oneMinusCos * zx) - ys;
		pMatrix->elements[2 * 4 + 1] = (oneMinusCos * yz) + xs;
		pMatrix->elements[2 * 4 + 2] = (oneMinusCos * zz) + cosAngle;
		pMatrix->elements[2 * 4 + 3] = 0.0F;

		pMatrix->elements[3 * 4 + 0] = 0.0F;
		pMatrix->elements[3 * 4 + 1] = 0.0F;
		pMatrix->elements[3 * 4 + 2] = 0.0F;
		pMatrix->elements[3 * 4 + 3] = 1.0F;
	}
}

void matrixMultiply(Matrix4x4* pMatrix, Matrix4x4* pA, Matrix4x4* pB)
{
	int i;

	for (i = 0; i < 4; i++)
	{
		pMatrix->elements[i * 4 + 0] = (pA->elements[i * 4 + 0] * pB->elements[0 * 4 + 0])
				+ (pA->elements[i * 4 + 1] * pB->elements[1 * 4 + 0])
				+ (pA->elements[i * 4 + 2] * pB->elements[2 * 4 + 0])
				+ (pA->elements[i * 4 + 3] * pB->elements[3 * 4 + 0]);

		pMatrix->elements[i * 4 + 1] = (pA->elements[i * 4 + 0] * pB->elements[0 * 4 + 1])
				+ (pA->elements[i * 4 + 1] * pB->elements[1 * 4 + 1])
				+ (pA->elements[i * 4 + 2] * pB->elements[2 * 4 + 1])
				+ (pA->elements[i * 4 + 3] * pB->elements[3 * 4 + 1]);

		pMatrix->elements[i * 4 + 2] = (pA->elements[i * 4 + 0] * pB->elements[0 * 4 + 2])
				+ (pA->elements[i * 4 + 1] * pB->elements[1 * 4 + 2])
				+ (pA->elements[i * 4 + 2] * pB->elements[2 * 4 + 2])
				+ (pA->elements[i * 4 + 3] * pB->elements[3 * 4 + 2]);

		pMatrix->elements[i * 4 + 3] = (pA->elements[i * 4 + 0] * pB->elements[0 * 4 + 3])
				+ (pA->elements[i * 4 + 1] * pB->elements[1 * 4 + 3])
				+ (pA->elements[i * 4 + 2] * pB->elements[2 * 4 + 3])
				+ (pA->elements[i * 4 + 3] * pB->elements[3 * 4 + 3]);
	}

}
